#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import pygame
import socket
import time
import threading
import logging
import sys
import os
import struct

# 配置日志
logging.basicConfig(level=logging.INFO, format='%(asctime)s - %(levelname)s - %(message)s')
logger = logging.getLogger(__name__)

class JoystickController:
    def __init__(self, server_host='127.0.0.1', server_port=8888, 
                 max_vx=1.0, max_vy=1.0, max_wz=1.0,
                 deadzone=0.05):
        """
        初始化游戏手柄控制器
        :param server_host: 服务器地址
        :param server_port: 服务器端口
        :param max_vx: X方向最大速度
        :param max_vy: Y方向最大速度
        :param max_wz: 最大角速度
        :param deadzone: 摇杆死区，小于此值的输入将被忽略
        """
        self.server_host = server_host
        self.server_port = server_port
        self.max_vx = max_vx
        self.max_vy = max_vy
        self.max_wz = max_wz
        self.deadzone = deadzone
        
        # 当前速度值
        self.vx = 0.0
        self.vy = 0.0
        self.wz = 0.0
        
        # Socket连接
        self.socket = None
        self.connected = False
        self.running = False
        
        # 游戏手柄
        self.joystick = None
        
        # 线程
        self.joystick_thread = None
    
    def connect(self):
        """
        连接到服务器
        """
        try:
            self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            self.socket.connect((self.server_host, self.server_port))
            self.connected = True
            logger.info(f"已连接到服务器 {self.server_host}:{self.server_port}")
            return True
        except Exception as e:
            logger.error(f"连接服务器失败: {e}")
            self.connected = False
            return False
    
    def setup_joystick(self):
        """
        设置游戏手柄
        """
        try:
            pygame.init()
            pygame.joystick.init()
            
            # 检查是否有可用的游戏手柄
            joystick_count = pygame.joystick.get_count()
            if joystick_count == 0:
                logger.error("未检测到游戏手柄，请确保手柄已连接")
                return False
            
            # 使用第一个检测到的游戏手柄
            self.joystick = pygame.joystick.Joystick(0)
            self.joystick.init()
            
            logger.info(f"已初始化游戏手柄: {self.joystick.get_name()}")
            logger.info(f"轴数量: {self.joystick.get_numaxes()}")
            logger.info(f"按钮数量: {self.joystick.get_numbuttons()}")
            
            return True
        except Exception as e:
            logger.error(f"设置游戏手柄失败: {e}")
            return False
    
    def start(self):
        """
        启动控制器
        """
        if not self.setup_joystick():
            return False
        
        if not self.connect():
            logger.warning("未能连接到服务器，将在后台尝试重连")
        
        self.running = True
        self.joystick_thread = threading.Thread(target=self._joystick_loop)
        self.joystick_thread.daemon = True
        self.joystick_thread.start()
        
        logger.info("游戏手柄控制器已启动")
        return True
    
    def _joystick_loop(self):
        """
        游戏手柄事件循环
        """
        try:
            while self.running:
                # 处理pygame事件
                pygame.event.pump()
                
                # 尝试重连
                if not self.connected:
                    self.connect()
                
                # 读取摇杆值
                if self.joystick:
                    # 左摇杆控制线速度 (vx, vy)
                    # 通常左摇杆是轴0(左右)和轴1(上下)
                    # x_axis = self.joystick.get_axis(0)  # 左右移动
                    y_axis = self.joystick.get_axis(1)  # 前后移动
                    
                    # 右摇杆控制角速度 (wz)
                    # 通常右摇杆的左右是轴2或轴3
                    rot_axis = self.joystick.get_axis(0)  # 旋转
                    
                    # 应用死区
                    # x_axis = 0.0 if abs(x_axis) < self.deadzone else x_axis
                    y_axis = 0.0 if abs(y_axis) < self.deadzone else y_axis
                    rot_axis = 0.0 if abs(rot_axis) < self.deadzone else rot_axis
                    
                    # 计算速度值 (注意：游戏手柄的y轴通常是反向的)
                    self.vx = -y_axis * self.max_vx  # 前进为正，后退为负
                    # self.vy = -x_axis * self.max_vy  # 左移为正，右移为负
                    self.wz = -rot_axis * self.max_wz  # 逆时针为正，顺时针为负
                    
                    # 检查急停按钮 (通常是按钮0，即A键或X键)
                    if self.joystick.get_button(0):
                        self.vx = 0.0
                        self.vy = 0.0
                        self.wz = 0.0
                        logger.info("紧急停止")
                    
                    # 发送控制命令
                    self._send_command()
                
                # 控制循环频率
                time.sleep(0.5)  # 2Hz
        except Exception as e:
            logger.error(f"游戏手柄循环出错: {e}")
        finally:
            if self.running:
                logger.info("游戏手柄循环意外终止，尝试重启")
                self.start()
    
    def _send_command(self):
        """发送控制命令"""
        if not self.connected:
            return
        
        try:
            # 基本命令格式 (字符串格式，与现有系统兼容)
            command_str = f"{self.vx:.2f},{self.vy:.2f},{self.wz:.2f}"
            
            # 发送命令 (使用简单的vx,vy,wz字符串格式)
            self.socket.sendall(command_str.encode('utf-8'))
            logger.info(f"已发送命令: {command_str}")
            
        except Exception as e:
            logger.error(f"发送命令失败: {e}")
            self.connected = False
    
    def stop(self):
        """
        停止控制器
        """
        self.running = False
        
        # 关闭Socket连接
        if self.socket:
            try:
                self.socket.close()
            except:
                pass
            self.socket = None
        
        # 关闭pygame
        try:
            pygame.quit()
        except:
            pass
        
        logger.info("游戏手柄控制器已停止")


if __name__ == "__main__":
    import argparse
    
    # 解析命令行参数
    parser = argparse.ArgumentParser(description="游戏手柄控制麦克纳姆轮底盘")
    parser.add_argument("--host", default="192.168.1.186", help="服务器地址")
    parser.add_argument("--port", type=int, default=8888, help="服务器端口")
    parser.add_argument("--max-vx", type=float, default=0.8, help="X方向最大速度")
    parser.add_argument("--max-vy", type=float, default=0.8, help="Y方向最大速度")
    parser.add_argument("--max-wz", type=float, default=0.8, help="最大角速度")
    parser.add_argument("--deadzone", type=float, default=0.05, help="摇杆死区")
    
    args = parser.parse_args()
    
    # 创建并启动控制器
    controller = JoystickController(
        server_host=args.host,
        server_port=args.port,
        max_vx=args.max_vx,
        max_vy=args.max_vy,
        max_wz=args.max_wz,
        deadzone=args.deadzone
    )
    
    try:
        if controller.start():
            print("游戏手柄控制器已启动，按Ctrl+C退出")
            # 保持主线程运行
            while True:
                time.sleep(1)
        else:
            print("游戏手柄控制器启动失败")
    except KeyboardInterrupt:
        print("程序被用户中断")
    finally:
        controller.stop()
